#!/usr/bin/env python
import roslib; roslib.load_manifest('teleop_twist_keyboard')
import rospy

from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy

import sys, select, termios, tty

msg = """
Reading from the keyboard  and Publishing to Twist!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
   U    I    O
   J    K    L
   M    <    >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
"""

moveBindings = {
    'i':(1,0,0,0),
    'o':(1,0,0,-1),
    'j':(0,0,0,1),
    'l':(0,0,0,-1),
    'u':(1,0,0,1),
    ',':(-1,0,0,0),
    '.':(-1,0,0,1),
    'm':(-1,0,0,-1),
    'O':(1,-1,0,0),
    'I':(1,0,0,0),
    'J':(0,1,0,0),
    'L':(0,-1,0,0),
    'U':(1,1,0,0),
    '<':(-1,0,0,0),
    '>':(-1,-1,0,0),
    'M':(-1,1,0,0),
    't':(0,0,1,0),
    'b':(0,0,-1,0),}

speedBindings={
    'q':(1.1,1.1),
    'z':(.9,.9),
    'w':(1.1,1),
    'x':(.9,1),
    'e':(1,1.1),
    'c':(1,.9),}

def getKey():
    tty.setraw(sys.stdin.fileno())
    select.select([sys.stdin], [], [], 0)
    key = sys.stdin.read(1)
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

fin_speed = 0.0
fin_turn = 0.5
speed = 0.0
turn = 0.0
joy_x = 0
joy_y = 0
joy_z = 0
joy_r = 0
joy_quit = 0

def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

def joy_callback(data):
    global joy_x, joy_y, joy_z, joy_r
    global speed, turn
    global joy_quit
    print "get a joy"
    if data.axes[2] < 1.0:
        speed = abs(data.axes[2] - 1) * 0.1 - fin_speed
        joy_x = -1
    else:
        if data.axes[5] < 1.0:
            speed = abs(data.axes[5] - 1) * 0.1 - fin_speed
            joy_x = 1
        else:
            speed = 0
            joy_x = 0
       
    if data.axes[0] < 0.0:  # turn right
        turn = abs(data.axes[0]) - fin_turn
        joy_r = -1
    if data.axes[0] > 0.0:  # turn left
        turn = abs(data.axes[0]) - fin_turn
        joy_r = 1

    if data.axes[3] < 0.0:  # around left
        turn = abs(data.axes[3]) - fin_turn
        joy_r = -1
        joy_x = 0
        joy_y = 0
        joy_z = 0
    if data.axes[3] > 0.0:  # around right
        turn = abs(data.axes[3]) - fin_turn
        joy_r = 1
        joy_x = 0
        joy_y = 0
        joy_z = 0
        
    if data.buttons[0] == 1:
        joy_x = 0
        joy_y = 0
        joy_z = 0
        joy_r = 0

    if data.buttons[1] == 1:
        joy_quit = 1
    print vels(speed,turn)

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)
    
    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
    rospy.Subscriber('joy', Joy, joy_callback)
    rospy.init_node('joystick_twist')

    x = 0
    y = 0
    z = 0
    th = 0
    status = 0

    try:
        #  print msg
        print vels(speed,turn)
        while(1):
            #  key = getKey()
            #  if key in moveBindings.keys():
                #  x = moveBindings[key][0]
                #  y = moveBindings[key][1]
                #  z = moveBindings[key][2]
                #  th = moveBindings[key][3]
            #  elif key in speedBindings.keys():
                #  speed = speed * speedBindings[key][0]
                #  turn = turn * speedBindings[key][1]

                #  print vels(speed,turn)
                #  if (status == 14):
                    #  print msg
                #  status = (status + 1) % 15
            #  else:
                #  x = 0
                #  y = 0
                #  z = 0
                #  th = 0
                #  if (key == '\x03'):
                    #  break
            if  joy_quit == 1:
                print 'Quit'
                break

            x = joy_x
            y = joy_y
            z = joy_z
            th = joy_r

            twist = Twist()
            twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn
            pub.publish(twist)

    except:
        print e

    finally:
        twist = Twist()
        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
        pub.publish(twist)

        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
